/*
 * control.cpp
 *
 *  Created on: Feb 19, 2014
 *      Author: bohni
 */

#include "control.h"

#define dt 		(ABTASTZEIT_IN_MS / 1000.0)

#define INTEGRATORLIMIT 10000

int8_t motor_values[4];
int8_t ctl_x = 0;
int8_t ctl_y = 0;
int8_t ctl_z = 0;

// int8_t regler_limit = REGLER_LIMIT_MIN;

// PID:

struct pid {
	uint8_t p;
	uint8_t i;
	uint8_t d;
	float e;
	float e_old;
	float e_sum;
};

static float tmp = 0;

struct pid pid_x_angle = { 0, 0, 0, 0, 0, 0 };
struct pid pid_y_angle = { 0, 0, 0, 0, 0, 0 };

struct pid pid_x_accel = { 0, 0, 0, 0, 0, 0 };
struct pid pid_y_accel = { 0, 0, 0, 0, 0, 0 };

struct pid pid_z_rate = { 0, 0, 0, 0, 0, 0 };

struct pid pid_z_angle = { 0, 0, 0, 0, 0, 0 };

void control(int8_t throttle, int8_t x, int8_t y, int8_t z) {

	if (throttle > THROTTLE_MAX) {
		throttle = THROTTLE_MAX;
	}

	if (throttle > REGLER_MIN) {

		// TODO:control XY
		control_angle_xy(throttle, (float) x / 2.0 , (float) y / 2.0);
		control_rate_z((float) z * Z_RATE_SENSITIVITY);

	} else {

		ctl_x = 0;
		ctl_y = 0;
		ctl_z = 0;


		// I Anteile leeren
		pid_x_accel.e_sum = 0;
		pid_y_accel.e_sum = 0;
		pid_x_accel.e_old = 0;
		pid_y_accel.e_old = 0;

		pid_x_angle.e_sum = 0;
		pid_y_angle.e_sum = 0;
		pid_x_angle.e_old = 0;
		pid_y_angle.e_old = 0;

		pid_z_angle.e_sum = 0;
		pid_z_rate.e_sum = 0;
		pid_z_angle.e_old = 0;
		pid_z_rate.e_old = 0;


	}
	ctl_z = 0;
	motor_values[0] = throttle - ctl_x - ctl_z;
	motor_values[1] = throttle + ctl_y + ctl_z;
	motor_values[2] = throttle + ctl_x - ctl_z;
	motor_values[3] = throttle - ctl_y + ctl_z;

}

void control_angle_xy(int8_t throttle, float w_angle_x, float w_angle_y) {

	// x :

	pid_x_angle.e = w_angle_x - x.angle;
	pid_x_angle.e_sum += pid_x_angle.e;

	if (pid_x_angle.e_sum > INTEGRATORLIMIT) {

		pid_x_angle.e_sum = INTEGRATORLIMIT;
	} else if (pid_x_angle.e_sum < -1 * INTEGRATORLIMIT) {

		pid_x_angle.e_sum = -1 * INTEGRATORLIMIT;
	}

	tmp =
			(float) pid_x_angle.p
					* pid_x_angle.e+ (float) pid_x_angle.i * pid_x_angle.e_sum * dt
					+ (float) pid_x_angle.d * ( pid_x_angle.e - pid_x_angle.e_old ) / dt;
	tmp /= 500.0;
	pid_x_angle.e_old = pid_x_angle.e;
	ctl_x = (int8_t) tmp;

	// X-LIMITS

	if (ctl_x > REGLER_LIMIT) {

		ctl_x = REGLER_LIMIT;
	} else if (ctl_x < 0) {

		if (-1 * ctl_x > REGLER_LIMIT) {

			ctl_x = -1 * REGLER_LIMIT;
		}
	}

	if ((throttle + ctl_x) < REGLER_MIN) {

		ctl_x = REGLER_MIN - throttle;
	} else if ((throttle - ctl_x) < REGLER_MIN) {

		ctl_x = throttle - REGLER_MIN;
	}

	////////
	// y:

	pid_y_angle.e = w_angle_y - y.angle;
	pid_y_angle.e_sum += pid_y_angle.e;

	if (pid_y_angle.e_sum > INTEGRATORLIMIT) {

		pid_y_angle.e_sum = INTEGRATORLIMIT;
	} else if (pid_y_angle.e_sum < -1 * INTEGRATORLIMIT) {

		pid_y_angle.e_sum = -1 * INTEGRATORLIMIT;
	}

	tmp =
			(float) pid_y_angle.p
					* pid_y_angle.e+ (float) pid_y_angle.i * pid_y_angle.e_sum * dt
					+ (float) pid_y_angle.d * ( pid_y_angle.e - pid_y_angle.e_old ) / dt;
	tmp /= 500.0;
	pid_y_angle.e_old = pid_y_angle.e;
	ctl_y = (int8_t) tmp;

	// X-LIMITS:

	if (ctl_y > REGLER_LIMIT) {

		ctl_y = REGLER_LIMIT;
	} else if (ctl_y < 0) {

		if (-1 * ctl_y > REGLER_LIMIT) {

			ctl_y = -1 * REGLER_LIMIT;
		}
	}

	if ((throttle + ctl_y) < REGLER_MIN) {

		ctl_y = REGLER_MIN - throttle;
	} else if ((throttle - ctl_y) < REGLER_MIN) {

		ctl_y = throttle - REGLER_MIN;
	}

}

void control_angle_z(float w_angle_z) {

	// TODO: Anpassen bei Uebergang bei 360grad

	pid_z_angle.e = w_angle_z - z.angle;
	pid_z_angle.e_sum += pid_z_angle.e;

	if (pid_z_angle.e_sum > INTEGRATORLIMIT) {

		pid_z_angle.e_sum = INTEGRATORLIMIT;
	} else if (pid_z_angle.e_sum < -1 * INTEGRATORLIMIT) {

		pid_z_angle.e_sum = -1 * INTEGRATORLIMIT;
	}

	tmp =
			(float) pid_z_angle.p
					* pid_z_angle.e+ (float) pid_z_angle.i * pid_z_angle.e_sum * dt
					+ (float) pid_z_angle.d * ( pid_z_angle.e - pid_z_angle.e_old ) / dt;
	tmp /= 500.0;
	pid_z_angle.e_old = pid_z_angle.e;

	control_rate_z(tmp);

}

void control_acceleration_xy(int8_t throttle, float w_acc_x, float w_acc_y) {
	float w_angle_x = 0;
	float w_angle_y = 0;

	pid_x_accel.e = w_acc_x - x.acceleration;
	pid_x_accel.e_sum += pid_x_accel.e;

	if (pid_x_accel.e_sum > INTEGRATORLIMIT) {

		pid_x_accel.e_sum = INTEGRATORLIMIT;
	} else if (pid_x_accel.e_sum < -1 * INTEGRATORLIMIT) {

		pid_x_accel.e_sum = -1 * INTEGRATORLIMIT;
	}

	tmp =
			(float) pid_x_accel.p
					* pid_x_accel.e+ (float) pid_x_accel.i * pid_x_accel.e_sum * dt
					+ (float) pid_x_accel.d * ( pid_x_accel.e - pid_x_accel.e_old ) / dt;

	tmp /= 500.0;
	pid_x_accel.e_old = pid_x_accel.e;

	//TODO : winkel berechnung

	pid_y_accel.e = w_acc_y - y.acceleration;
	pid_y_accel.e_sum += pid_y_accel.e;

	if (pid_y_accel.e_sum > INTEGRATORLIMIT) {

		pid_y_accel.e_sum = INTEGRATORLIMIT;
	} else if (pid_y_accel.e_sum < -1 * INTEGRATORLIMIT) {

		pid_y_accel.e_sum = -1 * INTEGRATORLIMIT;
	}

	tmp =
			(float) pid_y_accel.p
					* pid_y_accel.e+ (float) pid_y_accel.i * pid_y_accel.e_sum * dt
					+ (float) pid_y_accel.d * ( pid_y_accel.e - pid_y_accel.e_old ) / dt;

	tmp /= 500.0;
	pid_y_accel.e_old = pid_y_accel.e;

	//TODO : winkel berechnung

	control_angle_xy(throttle, w_angle_x, w_angle_y);
}

void control_rate_z(float w_rate_z) {

	pid_z_rate.e = w_rate_z - z.rate;
	pid_z_rate.e_sum += pid_z_rate.e;

	if (pid_z_rate.e_sum > INTEGRATORLIMIT) {

		pid_z_rate.e_sum = INTEGRATORLIMIT;
	} else if (pid_z_rate.e_sum < -1 * INTEGRATORLIMIT) {

		pid_z_rate.e_sum = -1 * INTEGRATORLIMIT;
	}

	tmp =
			(float) pid_z_rate.p
					* pid_z_rate.e+ (float) pid_z_rate.i * pid_z_rate.e_sum * dt
					+ (float) pid_z_rate.d * ( pid_z_rate.e - pid_z_rate.e_old ) / dt;
	tmp /= 500.0;
	pid_z_rate.e_old = pid_z_rate.e;

	if (tmp > Z_LIMIT) {
		tmp = Z_LIMIT;
	} else if (tmp < 0) {
		if (-1 * tmp > Z_LIMIT) {
			tmp = -1 * Z_LIMIT;
		}
	}

	ctl_z = (int8_t) tmp;

}

void control_v_xy(int8_t throttle, float w_v_x, float w_v_y) {
	// TODO control_v_xy
}

void control_init() {
	//Auslesen der Reglerwerte
	pid_x_angle.p = eeprom_read_byte((uint8_t *) EEPROM_ADR_P_XY_ANGLE);
	pid_y_angle.p = pid_x_angle.p;
	pid_x_angle.i = eeprom_read_byte((uint8_t *) EEPROM_ADR_I_XY_ANGLE);
	pid_y_angle.i = pid_x_angle.i;
	pid_x_angle.d = eeprom_read_byte((uint8_t *) EEPROM_ADR_D_XY_ANGLE);
	pid_y_angle.d = pid_x_angle.d;

	pid_x_accel.p = eeprom_read_byte((uint8_t *) EEPROM_ADR_P_XY_ACCEL);
	pid_y_accel.p = pid_x_accel.p;
	pid_x_accel.i = eeprom_read_byte((uint8_t *) EEPROM_ADR_I_XY_ACCEL);
	pid_y_accel.i = pid_x_accel.i;
	pid_x_accel.d = eeprom_read_byte((uint8_t *) EEPROM_ADR_D_XY_ACCEL);
	pid_y_accel.d = pid_x_accel.d;

	pid_z_rate.p = eeprom_read_byte((uint8_t *) EEPROM_ADR_P_Z_RATE);
	pid_z_rate.i = eeprom_read_byte((uint8_t *) EEPROM_ADR_I_Z_RATE);
	pid_z_rate.d = eeprom_read_byte((uint8_t *) EEPROM_ADR_D_Z_RATE);

	pid_z_angle.p = eeprom_read_byte((uint8_t *) EEPROM_ADR_P_Z_ANGLE);
	pid_z_angle.i = eeprom_read_byte((uint8_t *) EEPROM_ADR_I_Z_ANGLE);
	pid_z_angle.d = eeprom_read_byte((uint8_t *) EEPROM_ADR_D_Z_ANGLE);

}

void control_pid_update() {
	// TODO alle weiteren PID werte
	system_pause();
	sei();
	// activate interrupts for uart-receiver

	send_buffer[0] = pid_x_angle.p;
	send_buffer[1] = pid_x_angle.i;
	send_buffer[2] = pid_x_angle.d;

	send_buffer[3] = pid_z_rate.p;
	send_buffer[4] = pid_z_rate.i;
	send_buffer[5] = pid_z_rate.d;

	UART_send(6);

	while (received_number < 6)
		;

	eeprom_update_byte((uint8_t *) EEPROM_ADR_P_XY_ANGLE, receive_buffer[0]);
	eeprom_update_byte((uint8_t *) EEPROM_ADR_I_XY_ANGLE, receive_buffer[1]);
	eeprom_update_byte((uint8_t *) EEPROM_ADR_D_XY_ANGLE, receive_buffer[2]);

	eeprom_update_byte((uint8_t *) EEPROM_ADR_P_Z_RATE, receive_buffer[3]);
	eeprom_update_byte((uint8_t *) EEPROM_ADR_I_Z_RATE, receive_buffer[4]);
	eeprom_update_byte((uint8_t *) EEPROM_ADR_D_Z_RATE, receive_buffer[5]);

	system_reset();
}
